﻿using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

namespace SuperBall3D
{
    public abstract class DynamicSuperBallObject : SuperBallObject
    {
        // Includes player and boxes
        Vector3 previousPosition;
        Vector3 acceleration =  Vector3.Zero;
        Vector3 velocity = Vector3.Zero;
        float mass = 1.0f;
        protected float max_speed;
        bool colliding;

        public DynamicSuperBallObject(Game g, bool centerOnBot, Vector3 pos, float mass, float friction, float elas)
            : base(g, centerOnBot, friction, elas)
        {
            previousPosition = pos;
            dynamicSuperBallObjects.Add(this);
            this.mass = mass;
            
        }

        protected override void LoadContent()
        {
            base.LoadContent();
        }

        public override void Update(GameTime gameTime)
        {
            acceleration += Physics.gVector;
            Vector3 newVelo = Velocity + Acceleration * Physics.deltaT;
            
            if (newVelo.Length() > max_speed)
            {
                newVelo.Normalize();
                newVelo *= max_speed;
            }
            Vector3 newPos = Position + Velocity*Physics.deltaT +
                0.5f*Acceleration*Physics.deltaTSq;
            previousPosition = Position;
            Position = newPos;

            Velocity = newVelo;

            Geo.UpdateGeometry(Position);

            acceleration = Vector3.Zero;
            base.Update(gameTime);

        }


        public override void Draw(GameTime gameTime)
        {
            base.Draw(gameTime);
        }

        public Vector3 Momentum
        {
            get
            {
                return velocity * mass;
            }
        }

        public Vector3 Acceleration
        {
            get
            {
                return acceleration;
            }
            protected set
            {
                acceleration = value;
            }
        }

        public float Mass
        {
            get
            {
                return mass;
            }
            protected set
            {
                mass = value;
            }
        }

        public Vector3 Velocity
        {
            get
            {
                return velocity;
            }
            protected set
            {
                velocity = value;
            }
        }

        protected Vector3 PreviousPosition
        {
            get
            {
                return previousPosition;
            }
        }

        public float KineticEnergy
        {
            get
            {
                return velocity.LengthSquared() * mass * 0.5f;
            }
        }

        public bool Colliding
        {
            get
            {
                return colliding;
            }

            set
            {
                colliding = value;
            }
        }


        public override void HandleCollisionWithDynamic(DynamicSuperBallObject obj, Vector3 thisVelocity, Vector3 otherVelocity, Vector3 contactNormal, float penetrationDepth)
        {
            contactNormal = - contactNormal; //switch what we're passing this
            float thisVelScalar = Vector3.Dot(thisVelocity, contactNormal);
            float otherVelScalar = Vector3.Dot(otherVelocity, contactNormal);
            float thisMass = this.Mass;
            float otherMass = obj.Mass;

            //subtract out previous velocity in collision direction
            Vector3 newThisVel = thisVelocity - thisVelScalar * contactNormal;
            Vector3 newOtherVel = otherVelocity - otherVelScalar * contactNormal; 

            //scale for friction in non-collision direction

            newThisVel *= obj.FrictionCoefficient * this.FrictionCoefficient;
            newOtherVel *= obj.FrictionCoefficient * this.FrictionCoefficient;

            //calculate one-dimensional velocity account for elasticity (should probably both be multiplied by each one, like friction
            float newThisVelScalar = this.Elasticity * (thisVelScalar * (thisMass - otherMass) + (2 * otherMass * otherVelScalar)) / (thisMass + otherMass);

            float newOtherVelScalar = obj.Elasticity * (otherVelScalar * (otherMass - thisMass) + (2 * thisMass * thisVelScalar)) / (thisMass + otherMass);

            //add in new velocity
            newThisVel += newThisVelScalar * contactNormal; //should this be + or -?
            newOtherVel += newOtherVelScalar * contactNormal;

            //set it
            this.Velocity = newThisVel;
            obj.Velocity = newOtherVel;

            //separate to end collision
            Position -= contactNormal * penetrationDepth;

            if (this is Player && (contactNormal == -Vector3.UnitY))
                (this as Player).ReadyToJump = true;
            if (obj is Player && (contactNormal == -Vector3.UnitY))
                (obj as Player).ReadyToJump = true;
        }



        public override void HandleCollisionWithStatic(Vector3 contactNormal, float penetrationDepth, float otherObjFriction)
        {
            Vector3 velocity_new = Vector3.Reflect(this.Velocity, contactNormal);

            velocity_new = CollisionVelocity(velocity_new, contactNormal, Elasticity, FrictionCoefficient * otherObjFriction); //.7 replace with elasticity of some kind
            if (this is Player && (contactNormal == Vector3.UnitY)) 
                (this as Player).ReadyToJump = true;

            Position += contactNormal * penetrationDepth;
            this.Velocity = velocity_new;
        }

        Vector3 CollisionVelocity(Vector3 preCollisionVelocity, Vector3 contactNormal, float scaleFactorNormal, float scaleFactorSurface)
        {
            float axisAlignment;
            Vector3 postCollsionVelocity = preCollisionVelocity;

            axisAlignment = Math.Abs(Vector3.Dot(contactNormal, Vector3.UnitX));
            postCollsionVelocity.X *= scaleFactorNormal;
            postCollsionVelocity.Y *= scaleFactorSurface;
            postCollsionVelocity.Z *= scaleFactorSurface;

            if (axisAlignment < Math.Abs(Vector3.Dot(contactNormal, Vector3.UnitY)))
            {
                axisAlignment = Math.Abs(Vector3.Dot(contactNormal, Vector3.UnitY));
                postCollsionVelocity = preCollisionVelocity;
                postCollsionVelocity.Y *= scaleFactorNormal;
                postCollsionVelocity.X *= scaleFactorSurface;
                postCollsionVelocity.Z *= scaleFactorSurface;
            }

            if (axisAlignment < Math.Abs(Vector3.Dot(contactNormal, Vector3.UnitZ)))
            {
                axisAlignment = Math.Abs(Vector3.Dot(contactNormal, Vector3.UnitZ));
                postCollsionVelocity = preCollisionVelocity;
                postCollsionVelocity.Z *= scaleFactorNormal;
                postCollsionVelocity.X *= scaleFactorSurface;
                postCollsionVelocity.Y *= scaleFactorSurface;
            }

            return postCollsionVelocity;
      }

    }

}
